#include "window.h"
#include "ui_window.h"
#include <QPixmap>

Window::Window(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::Window)
{
    setFixedSize(1250, 700);
    setWindowTitle("IroBomb");


    ui->setupUi(this);
    m_sketch = new SketchWidget(this);
    m_sketch->setWindow(this);

    playButton = true;

    ui->openGLLayout->addWidget(m_sketch);
    connect(ui->nextStepButton,SIGNAL(clicked()),this, SLOT(doOneStep()));
    connect(ui->playButton,SIGNAL(clicked()),this, SLOT(doAll()));

    qCursor = new QCursor();

	this->com = NULL;
	this->aiboWalk = NULL;
	this->aiboHead = NULL;
	this->m_aiboState = NULL;
	this->aiboWalkTimer = NULL;
	this->aiboHeadTimer = NULL;
	this->keyWalkTimer = NULL;
	this->keyHeadTimer = NULL;
	this->camera = NULL;
	this->waitForConnect = NULL;
    this->walkForwardTimer = NULL;
    this->walkRotateTimer = NULL;
	this->m_walkHandler = NULL;
	this->m_headHandler = NULL;

	ui->ipAddress->setEnabled( true );
	ui->ipAddress->setText( "192.168.0.100");
	connect( ui->ipAddress, SIGNAL( returnPressed() ), this, SLOT( connectToAibo() ) );

	this->waitForConnect = new QTimer(this);
	connect( waitForConnect, SIGNAL(timeout()), this, SLOT( initAibo() ) ) ;
	ui->ipAddress->setFocus();
	this->setFocusPolicy( Qt::TabFocus );
	ui->rollSlider->setFocusPolicy( Qt::NoFocus );
	ui->panSlider->setFocusPolicy( Qt::NoFocus );
	ui->tiltSlider->setFocusPolicy( Qt::NoFocus );
	ui->playButton->setFocusPolicy( Qt::NoFocus );
	ui->nextStepButton->setFocusPolicy( Qt::NoFocus );
	this->m_sketch->setFocusPolicy( Qt::NoFocus );

    ui->labelImage->setPixmap(QPixmap::fromImage(QImage(600, 400, QImage::Format_RGB32)));
    ui->labelImage->show();
}

Window::~Window()
{
	disconnectAibo();
    delete ui;
}

void Window::setHeadKeyTimers() {
	keyHeadTimer = new QTimer( this );
	headKeyList.rollUp = new KeyPressTimer;
	headKeyList.rollDown = new KeyPressTimer;
	headKeyList.panLeft = new KeyPressTimer;
	headKeyList.panRight = new KeyPressTimer;
	headKeyList.tiltUp = new KeyPressTimer;
	headKeyList.tiltDown = new KeyPressTimer;
	connect( keyHeadTimer, SIGNAL(timeout()), this, SLOT(checkHeadControlKeys()) ) ;
	setKeyTimer( headKeyList.rollUp, Qt::Key_W, keyHeadTimer );
	setKeyTimer( headKeyList.rollDown, Qt::Key_S, keyHeadTimer );
	setKeyTimer( headKeyList.panLeft, Qt::Key_A, keyHeadTimer );
	setKeyTimer( headKeyList.panRight, Qt::Key_D, keyHeadTimer );
	setKeyTimer( headKeyList.tiltUp, Qt::Key_A, keyHeadTimer );
	setKeyTimer( headKeyList.tiltDown, Qt::Key_D, keyHeadTimer );
}

void Window::setWalkKeyTimers() 
{
	keyWalkTimer = new QTimer( this );
	walkKeyList.forwardWalk = new KeyPressTimer;
	walkKeyList.backwardWalk = new KeyPressTimer;
	walkKeyList.turnLeftWalk = new KeyPressTimer;
	walkKeyList.turnRightWalk = new KeyPressTimer;
	walkKeyList.strafeLeftWalk = new KeyPressTimer;
	walkKeyList.strafeRightWalk = new KeyPressTimer;
	connect( keyWalkTimer, SIGNAL(timeout()), this, SLOT(checkWalkControlKeys()) ) ;
	setKeyTimer( walkKeyList.forwardWalk, Qt::Key_I, keyWalkTimer );
	setKeyTimer( walkKeyList.backwardWalk, Qt::Key_K, keyWalkTimer );
	setKeyTimer( walkKeyList.turnLeftWalk, Qt::Key_J, keyWalkTimer );
	setKeyTimer( walkKeyList.turnRightWalk, Qt::Key_L, keyWalkTimer );
	setKeyTimer( walkKeyList.strafeLeftWalk, Qt::Key_J, keyWalkTimer );
	setKeyTimer( walkKeyList.strafeRightWalk, Qt::Key_L, keyWalkTimer );
}

void Window::checkWalkKeyTimers() 
{
	if( walkKeyList.forwardWalk != NULL && !walkKeyList.forwardWalk->isKeyPressed )
		if( walkKeyList.backwardWalk != NULL && !walkKeyList.backwardWalk->isKeyPressed )
			if( walkKeyList.turnLeftWalk != NULL && !walkKeyList.turnLeftWalk->isKeyPressed )
				if( walkKeyList.turnRightWalk != NULL && !walkKeyList.turnRightWalk->isKeyPressed )
					if( walkKeyList.strafeLeftWalk != NULL && !walkKeyList.strafeLeftWalk->isKeyPressed )
						if( walkKeyList.strafeRightWalk != NULL && !walkKeyList.strafeRightWalk->isKeyPressed )
							keyWalkTimer->stop();
}

void Window::checkHeadKeyTimers() 
{
	if( headKeyList.rollUp != NULL && !headKeyList.rollUp->isKeyPressed )
		if( headKeyList.rollDown != NULL && !headKeyList.rollDown->isKeyPressed )
			if( headKeyList.panLeft != NULL && !headKeyList.panLeft->isKeyPressed )
				if( headKeyList.panRight != NULL && !headKeyList.panRight->isKeyPressed )
					if( headKeyList.tiltUp != NULL && !headKeyList.tiltUp->isKeyPressed )
						if( headKeyList.tiltDown != NULL && !headKeyList.tiltDown->isKeyPressed )
                            keyHeadTimer->stop();
}

void Window::updateHeadStateWindow()
{
    ui->rollSlider->setValue((int)(this->aiboHead->getRoll()*100));
    ui->panSlider->setValue((int)(this->aiboHead->getPan()*100));
    ui->tiltSlider->setValue((int)(this->aiboHead->getTilt()*100));

    double angleFov = this->aiboHead->getPan() * 89 * 3.14/180;
    m_sketch->rotateFov(angleFov);
    m_sketch->updateWindow();
}

void Window::setKeyTimer( KeyPressTimer* kTimer, int keyPressed, QTimer* timer )
{
	if( kTimer != NULL ) {
		kTimer->isKeyPressed = false;
		kTimer->m_key = keyPressed;
		kTimer->keyTimer = timer;
	}
}

void Window::startWalkKeyTimer( KeyPressTimer* kTimer )
{
	qDebug() << "Window: Walk Key Pressed - Walk Timer Started";
	kTimer->isKeyPressed = true;
	if( !kTimer->keyTimer->isActive() )
		kTimer->keyTimer->start(1000);
}

void Window::startHeadKeyTimer( KeyPressTimer* kTimer ) {
	qDebug() << "Window: Head Key Pressed - Head Timer Started";
	kTimer->isKeyPressed = true;
	if( !kTimer->keyTimer->isActive() )
		kTimer->keyTimer->start(25);
}

void Window::stopWalkKeyTimer( KeyPressTimer* kTimer )
{
	kTimer->isKeyPressed = false;
	checkWalkKeyTimers();
}

void Window::stopHeadKeyTimer( KeyPressTimer* kTimer ) {
	kTimer->isKeyPressed = false;
	checkHeadKeyTimers();
}

void Window::connectToAibo() {
	if( !ui->ipAddress->text().isEmpty() ) {
		qDebug() << "Window: IP Address Entered";
		QHostAddress ipAddress = QHostAddress( ui->ipAddress->text() );
		if( ipAddress.protocol() == QAbstractSocket::IPv4Protocol ) {
			com = new Communication(this);
			this->m_aiboState = new AiboState();
			com->connectToAibo(ui->ipAddress->text(), m_aiboState);
			this->waitForConnect->start(1000);
		}
		ui->ipAddress->setEnabled( false );
		qDebug() << "Window: Connecting to Aibo";
		this->setFocus();
	}
}

void Window::checkWalkControlKeys() {
	qDebug() << "Window: Walk Key Held";
	if( walkKeyList.forwardWalk->isKeyPressed )
		this->aiboWalk->walkForward();
	if( walkKeyList.backwardWalk->isKeyPressed )
		this->aiboWalk->walkBackward();
	if( walkKeyList.turnLeftWalk->isKeyPressed )
		this->aiboWalk->turnLeft();
	if( walkKeyList.turnRightWalk->isKeyPressed )
		this->aiboWalk->turnRight();
	if( walkKeyList.strafeLeftWalk->isKeyPressed )
		this->aiboWalk->strafeLeft();
	if( walkKeyList.strafeRightWalk->isKeyPressed )
		this->aiboWalk->strafeRight();
}

void Window::checkHeadControlKeys() {
	qDebug() << "Window: Head Key Held";
	if( headKeyList.rollUp->isKeyPressed )
		this->aiboHead->rollUp();
	if( headKeyList.rollDown->isKeyPressed )
		this->aiboHead->rollDown();
	if( headKeyList.panLeft->isKeyPressed )
		this->aiboHead->panLeft();
	if( headKeyList.panRight->isKeyPressed )
		this->aiboHead->panRight();
	if( headKeyList.tiltUp->isKeyPressed )
		this->aiboHead->tiltUp();
	if( headKeyList.tiltDown->isKeyPressed )
        this->aiboHead->tiltDown();
}

void Window::disconnectAibo() {
	if( com != NULL && com->isTcpConnected() ) {
		com->closeConnection();
		while( com->isTcpConnected() ) {}
		delete com;
		com = NULL;
	}
	if( this->aiboHeadTimer != NULL && this->aiboHeadTimer->isActive() ) {
		this->aiboHeadTimer->stop();
		delete this->aiboHeadTimer;
		this->aiboHeadTimer = NULL;
	}
	if( this->aiboWalkTimer != NULL && this->aiboWalkTimer->isActive() ) {
		this->aiboWalkTimer->stop();
		delete this->aiboWalkTimer;
		this->aiboWalkTimer = NULL;
	}
	if( this->camera != NULL )
		this->camera->stop();

	if( this->m_walkHandler ) {
		qDebug() << "Window: Deleting Walk Handler";
		delete this->m_walkHandler;
		this->m_walkHandler = NULL;
	}
	if( aiboWalk ) {
		qDebug() << "Window: Deleting Walk Control";
		delete this->aiboWalk;
		this->aiboWalk = NULL;
	}
	if( this->m_headHandler ) {
		qDebug() << "Window: Deleting Head Handler";
		delete this->m_headHandler;
		this->m_headHandler = NULL;
	}
	if( aiboHead ) {
		qDebug() << "Window: Deleting HeadControl";
		delete this->aiboHead;
		this->aiboHead = NULL;
	}
	if( this->waitForConnect->isActive() ) {
		qDebug() << "Window: Ending Wait For Connect";
		this->waitForConnect->stop();
	}
	if( this->m_aiboState ) {
		delete this->m_aiboState;
		this->m_aiboState = NULL;
	}
	if( ui )
		ui->ipAddress->setFocus();
}

void Window::initAibo() {
    qDebug() << "Window: Checking Connection";
    if( com->isTcpConnected() && com->isHeadConnected() && com->isWalkConnected() && com->isCameraConnected()) {
		this->waitForConnect->stop();
		if( !this->m_walkHandler )
			this->m_walkHandler = new WalkInstructionHandler( m_aiboState );
		if( !this->m_headHandler )
			this->m_headHandler = new HeadInstructionHandler( m_aiboState );
		aiboWalk = new AiboWalkControl( m_walkHandler );
		aiboHead = new AiboHeadControl( m_headHandler );
		aiboWalk->setWalkSpeed(.30f);
		aiboWalk->setTurnSpeed(.30f);
		qDebug() << "Window: Aibo Connection";
		if( !this->aiboWalkTimer ) {
			this->aiboWalkTimer = new QTimer(this);
			connect( aiboWalkTimer, SIGNAL( timeout() ), this, SLOT( updateWalkInstructions() ) );
		}
		this->aiboWalkTimer->start( 25 ); 

		if( !this->aiboHeadTimer ) {
			this->aiboHeadTimer = new QTimer(this);
			connect( aiboHeadTimer, SIGNAL( timeout() ), this, SLOT( updateHeadInstructions() ) );
		}
		this->aiboHeadTimer->start( 25 ); 

		this->camera = new QTimer(this);
		connect( camera, SIGNAL(timeout()), this, SLOT(updateCamera()) ) ;
		this->camera->start(50);

		setWalkKeyTimers();
		setHeadKeyTimers();
		this->setFocus();
	}
}

void Window::updateWalkInstructions() {
	this->m_walkHandler->updateAiboState();
}

void Window::updateHeadInstructions() {
	this->m_headHandler->updateAiboState();
    updateHeadStateWindow();
}

bool Window::checkWalkKeyPressed( QKeyEvent *event ) {
    qDebug() << "Window: Checking Walk Controls";
	bool walkControlSelected = event->isAutoRepeat();
	if( !walkControlSelected ) {
		switch( event->key() ) {
			case Qt::Key_I:
				qDebug() << "Window: Aibo moving forward";
				this->aiboWalk->walkForward();
				startWalkKeyTimer( walkKeyList.forwardWalk );
				break;
			case Qt::Key_K:
				qDebug() << "Window: Aibo moving Backward";
				this->aiboWalk->walkBackward();
				startWalkKeyTimer( walkKeyList.backwardWalk );
				break;
			case Qt::Key_J:
 				if( event->modifiers() == Qt::ShiftModifier ) {
					qDebug() << "Window: Aibo Left Strafe";
					this->aiboWalk->strafeLeft();
					startWalkKeyTimer( walkKeyList.strafeLeftWalk );
				}
				else {
					qDebug() << "Window: Aibo Left turning";
					this->aiboWalk->turnLeft();
					startWalkKeyTimer( walkKeyList.turnLeftWalk );
				}
				break;
			case Qt::Key_L:
				if( event->modifiers() == Qt::ShiftModifier ) {
					qDebug() << "Window: Aibo Right Strafe";
					this->aiboWalk->strafeRight();
					startWalkKeyTimer( walkKeyList.strafeRightWalk );
				}
				else {
					qDebug() << "Window: Aibo Right turn\n";
					this->aiboWalk->turnRight();
					startWalkKeyTimer( walkKeyList.turnRightWalk );
				}
				break;
			default:
				qDebug() << "Window: Walk Control Not Requested";
				walkControlSelected = false;
				break;
		}
	}
	return walkControlSelected;
}

bool Window::checkHeadKeyPressed( QKeyEvent *event ) {
    qDebug() << "Window: Checking Head Controls";
	bool headControlSelected = event->isAutoRepeat();
	if( !headControlSelected ) {
		switch( event->key() ) {
		case Qt::Key_W:
			if( ! (event->modifiers() == Qt::ShiftModifier) ) {
				qDebug() << "Window: Aibo Rolling Up";
				this->aiboHead->rollUp();
				startHeadKeyTimer( headKeyList.rollUp );
			}
			else {
				qDebug() << "Window: Aibo Tilting Up";
				this->aiboHead->tiltUp();
				startHeadKeyTimer( headKeyList.tiltUp );
			}
			break;
		case Qt::Key_S:
			if( ! (event->modifiers() == Qt::ShiftModifier) ) {
				qDebug() << "Window: Aibo Rolling Down";
				this->aiboHead->rollDown();
				startHeadKeyTimer( headKeyList.rollDown );
			}
			else {
				qDebug() << "Window: Aibo Tilting Down";
				this->aiboHead->tiltDown();
				startHeadKeyTimer( headKeyList.tiltDown );
			}
			break;
		case Qt::Key_A:
			qDebug() << "Window: Aibo Panning Left";
			this->aiboHead->panLeft();
			startHeadKeyTimer( headKeyList.panLeft );
			break;
		case Qt::Key_D:
			qDebug() << "Window: Aibo Panning Right";
			this->aiboHead->panRight();
			startHeadKeyTimer( headKeyList.panRight );
			break;
		case Qt::Key_C:
			qDebug() << "Window: Aibo Centering Head";
			this->aiboHead->centerHead();
			break;
		}
	}
    return headControlSelected;
}

bool Window::checkWalkKeyReleased( QKeyEvent *event ) 
{
	bool isRepeated = event->isAutoRepeat();
	if( !isRepeated ) {
		if( event->key() == Qt::Key_I ) {
			qDebug() << "Window: Aibo stopping forward";
			this->aiboWalk->stopWalk();
			stopWalkKeyTimer( walkKeyList.forwardWalk );
		}
		else if( event->key() == Qt::Key_K ) {
			qDebug() << "Window: Aibo stopping Backward";
			this->aiboWalk->stopWalk();
			stopWalkKeyTimer( walkKeyList.backwardWalk );
		}
		else if( event->key() == Qt::Key_J ) {
			if( event->modifiers() == Qt::ShiftModifier ) {
				qDebug() << "Window: Aibo Left Strafe Done";
				this->aiboWalk->stopStrafe();
				stopWalkKeyTimer( walkKeyList.strafeLeftWalk );
			}
			else {
				qDebug() << "Window: Aibo Left Turn Done";
				this->aiboWalk->stopTurn();
				stopWalkKeyTimer( walkKeyList.turnLeftWalk );
			}
		}
		else if( event->key() == Qt::Key_L ) {
			if( event->modifiers() == Qt::ShiftModifier ) {
				qDebug() << "Window: Aibo Right Strafe Done";
				this->aiboWalk->stopStrafe();
				stopWalkKeyTimer( walkKeyList.strafeRightWalk );
			}
			else {
				qDebug() << "Window: Aibo Right Turn Done";
				this->aiboWalk->stopTurn();
				stopWalkKeyTimer( walkKeyList.turnRightWalk );
			}
		}
		else if( event->modifiers() == Qt::ShiftModifier ) {
			if( event->key() == Qt::Key_J ) {
				qDebug() << "Window: Aibo Strafe Left Done";
				this->aiboWalk->stopStrafe();
				stopWalkKeyTimer( walkKeyList.strafeLeftWalk );
			}
			else if( event->key() == Qt::Key_L ) {
				qDebug() << "Window: Aibo Strafe Right Done";
				this->aiboWalk->stopStrafe();
				stopWalkKeyTimer( walkKeyList.strafeRightWalk );
			}
		}
	}
	return isRepeated;
}

bool Window::checkHeadKeyReleased( QKeyEvent *event ) {
    qDebug() << "Window: Checking Head Controls";
	bool headControlSelected = event->isAutoRepeat();
	if( !headControlSelected ) {
		switch( event->key() ) {
		case Qt::Key_W:
			if( ! (event->modifiers() == Qt::ShiftModifier) ) {
				qDebug() << "Window: Aibo Rolling Up Done";
				stopHeadKeyTimer( headKeyList.rollUp );
			}
			else {
				qDebug() << "Window: Aibo Tilting Up Done";
				stopHeadKeyTimer( headKeyList.tiltUp );
			}
			break;
		case Qt::Key_S:
			if( ! (event->modifiers() == Qt::ShiftModifier) ) {
				qDebug() << "Window: Aibo Rolling Down Done";
				stopHeadKeyTimer( headKeyList.rollDown );
			}
			else {
				qDebug() << "Window: Aibo Tilting Down Done";
				stopHeadKeyTimer( headKeyList.tiltDown );
			}
			break;
		case Qt::Key_A:
			qDebug() << "Window: Aibo Panning Left Done";
			stopHeadKeyTimer( headKeyList.panLeft );
			break;
		case Qt::Key_D:
			qDebug() << "Window: Aibo Panning Right Done";
			stopHeadKeyTimer( headKeyList.panRight );
			break;
		}
	}
    return headControlSelected;
}

void Window::keyPressEvent( QKeyEvent *event )
{
	if( this->com ) {
		if( event->key() == Qt::Key_X ) {
				qDebug() << "Window: Ending Connection";
				disconnectAibo();
				ui->ipAddress->setEnabled( true );
		}
		else if( this->com->isTcpConnected() && !checkWalkKeyPressed( event ) )
			checkHeadKeyPressed( event );
	}
}

void Window::keyReleaseEvent( QKeyEvent *event ) {
	if( this->com && this->com->isTcpConnected() ) {
		qDebug() << "Window: Key Released";
		if( !checkWalkKeyReleased( event ) ) 
			checkHeadKeyReleased( event );
	}
}

void Window::mouseMoveEvent(QMouseEvent *event) {
	this->qCursor->setPos( event->globalPos() );
	qDebug() << "MouseX: " << this->qCursor->pos().x() << " MouseY: " << this->qCursor->pos().y();
}

void Window::doOneStep()
{
	qDebug() << "Next Step Requested";
	if( this->com != NULL ) {
		//We create the instructions for the first point
		createInstructionsForFirstPoint();

		//We read the instruction list
		readInstructions();
	}
}

void Window::doAll()
{
	qDebug() << "Play Requested";
	if( this->com ) {

		if (playButton) {
			//We change the button
			playButton = false;
			ui->playButton->setText("Pause");

			//We create the instructions for each points
			createInstructionsForAllPoint();

			//We read the instruction list
			readInstructions();
		} else {
			//We change the button
			playButton = true;
			ui->playButton->setText("Play");

			//We delete the list of instruction
			listInstruction.remove(0, listInstruction.size());
		}
	}
}

void Window::doBodyRotation(double angle)
{
	if( this->com ) {
        //We change the mode label
        ui->modeLabel->setText("Rotation Mode : On");

		//We create the instruction
		createInstructionRotation(angle);

		//We read the instruction list
		readInstructions();
	}
}


void Window::doInstruction(WalkRotateInstruction* I) {
    currentInstruction = I;

    if ( this->walkRotateTimer == NULL) {
        this->walkRotateTimer = new QTimer(this);
        connect( walkRotateTimer, SIGNAL(timeout()), this, SLOT(doRotate()) ) ;
    }
    this->walkRotateTimer->start(1000);
    QTimer::singleShot(I->getDuration(), this, SLOT(finishRotate()));

}

void Window::doRotate() {
	if( this->com ) {
		this->aiboWalk->setTurnSpeed(currentInstruction->getValue());

		if (currentInstruction->getRealValue() > 0 ) {
			this->aiboWalk->turnLeft();
		} else {
			this->aiboWalk->turnRight();
		}
	}
}

void Window::finishRotate() {
    this->walkRotateTimer->stop();
    finishInstruction((WalkRotateInstruction*) currentInstruction );
}

void Window::finishInstruction(WalkRotateInstruction* I) {

    //Stop turning
    this->aiboWalk->stopTurn();

    //We do the rotation in the interface
    if ( m_sketch->getMode() == inRotation ) {
        m_sketch->setMode(nothing);
    } else {
        m_sketch->doRotate(I->getRealValue());
    }
    m_sketch->updateWindow();

    //We change the mode label
    ui->modeLabel->setText("Direction Mode : On");

    //We read the next instruction
    readInstructions();
}

void Window::doInstruction(WalkForwardInstruction* I) {

    currentInstruction = I;

    if ( this->walkForwardTimer == NULL) {
        this->walkForwardTimer = new QTimer(this);
        connect( walkForwardTimer, SIGNAL(timeout()), this, SLOT(doForward()) ) ;
    }
    this->walkForwardTimer->start(1000);
    QTimer::singleShot(I->getDuration(), this, SLOT(finishForward()));
}

void Window::doForward() {
    this->aiboWalk->setWalkSpeed(currentInstruction->getValue());

    this->aiboWalk->walkForward();
}

void Window::finishForward() {
    this->walkForwardTimer->stop();
    finishInstruction((WalkForwardInstruction*) currentInstruction );

}

void Window::finishInstruction(WalkForwardInstruction* I) {

    //Stop walking
    this->aiboWalk->stopWalk();

    //We do the walking in the interface
    m_sketch->doTranslate(I->getRealValue());
    m_sketch->updateWindow();

    //We delete the point
    m_sketch->deletePointIndice(1);
    m_sketch->updateWindow();

    //We read the next instruction
    readInstructions();
}


void Window::createInstructionsForFirstPoint()
{
    Point origin = Point(0.0, -0.70);
    Point ref = Point(0.0, 0.30);

    QVector<Point> listPoints = m_sketch->getPoints();
    QVector<Speed> listSpeed = m_sketch->getSpeeds();

    //We check if there is a path
    if ( listPoints.size() <= 1) {
        return;
    }

    //We take the Point
    Point p = listPoints[1];

    //We compute the angle for the rotation
    float angle = m_sketch->angle(ref-origin, p-origin);

    //We create the rotate instruction
    WalkRotateInstruction* rotateI = new WalkRotateInstruction(0.3,angleToDuration(angle), angle);
    //And we add it to the list
    listInstruction.push_back(rotateI);

    //We rotate the point
    p = m_sketch->rotatePoint(p,angle);

    //We compute the distance for walking
    float distance = sqrt((p.y() - origin.y())*(p.y() - origin.y()));

    //We create the walk instruction
    WalkForwardInstruction* forwardI = new WalkForwardInstruction(listSpeed[1]/10, distanceToDuration(distance), distance);
    //And we add it to the list
    listInstruction.push_back(forwardI);

}

void Window::createInstructionsForAllPoint() {
    Point origin = Point(0.0, -0.70);
    Point ref = Point(0.0, 0.30);

    QVector<Point> listPoints = m_sketch->getPoints();
    QVector<Speed> listSpeed = m_sketch->getSpeeds();
    int size = listPoints.size();

    for ( int i = 1; i < size; i++) {
        //We take the Point
        Point p = listPoints[1];

        //We compute the angle for the rotation
        float angle = m_sketch->angle(ref-origin, p-origin);

        //We create the rotate instruction
        WalkRotateInstruction* rotateI = new WalkRotateInstruction(0.3, angleToDuration(angle), angle);
        //And we add it to the list
        listInstruction.push_back(rotateI);

        //We rotate all points
        for (int j = 1; j < listPoints.size(); j++) {
            listPoints[j] = m_sketch->rotatePoint(listPoints[j],angle);
        }

        p = listPoints[1];

        //We compute the distance for walking
        float distance = sqrt((p.y() - origin.y())*(p.y() - origin.y()));
        int distanceToTime = 2000;

        //We create the walk instruction
        WalkForwardInstruction* forwardI = new WalkForwardInstruction(listSpeed[1]/10, distanceToTime, distance);

        //And we add it to the list
        listInstruction.push_back(forwardI);

        //We translate all points
        for (int j = 1; j < listPoints.size(); j++) {
            listPoints[j] = m_sketch->translatePoint(listPoints[j],distance);
        }

        //We remove the first point
        listPoints.remove(1);
        listSpeed.remove(1);

    }
}

void Window::createInstructionRotation(double angle)
{
    int angleToTime = 2000;

    //We create the rotate instruction
    WalkRotateInstruction* rotateI = new WalkRotateInstruction(.3f,angleToTime, angle);
    //And we add it to the list
    listInstruction.push_back(rotateI);

}

void Window::readInstructions()
{
    if ( !listInstruction.isEmpty() ) {
        //We take and we remove the instruction
        WalkInstruction* instruction = listInstruction.front();
        listInstruction.remove(0);

        switch( instruction->getType() ) {
        case CMD_WALK_FORWARD:
            doInstruction( (WalkForwardInstruction*)instruction );
            break;
        case CMD_WALK_ROTATE:
            doInstruction( (WalkRotateInstruction*)instruction );
            break;
        }
    } else {
        //We change the button
        playButton = true;
        ui->playButton->setText("Play");

    }
}


void Window::updateCamera()
{
    QImage tempImg = com->getCamera();
    //tempImg.save( "test.bmp", "BMP");
    ui->labelImage->setPixmap(QPixmap::fromImage(tempImg));
    ui->labelImage->show();
}

int Window::angleToDuration(float angle)
{
    return 2000;
}

int Window::distanceToDuration(float distance)
{
    return 2000;
}
